#include "flocking.h"
#include "../vehicle.h"
#include "../stages/ingame.h"
#include "../entitymanager.h"

#include <cassert>

using std::string;
using std::vector;


//------------------------- ctor -----------------------------------------
//
//------------------------------------------------------------------------
Flocking::Flocking(Vehicle* agent):
                                  
             
             m_pVehicle(agent),
             m_iFlags(0),
             m_dWeightCohesion(1.0),
             m_dWeightAlignment(1.0),
             m_dWeightSeparation(2.0),
             m_SummingMethod(prioritized)
{

}

//---------------------------------dtor ----------------------------------
Flocking::~Flocking()
{
}


/////////////////////////////////////////////////////////////////////////////// CALCULATE METHODS 


//----------------------- Calculate --------------------------------------
//
//  calculates the accumulated steering force according to the method set
//  in m_SummingMethod
//------------------------------------------------------------------------
Vector2D Flocking::Calculate()
{ 
  //reset the steering force
  m_vSteeringForce.Zero();

  switch (m_SummingMethod)
  {
  case weighted_average:
    
    m_vSteeringForce = CalculateWeightedSum(); break;

  case prioritized:

    m_vSteeringForce = CalculatePrioritized(); break;

  case dithered:
    
    m_vSteeringForce = CalculateDithered();break;

  default:m_vSteeringForce = Vector2D(0,0); 

  }//end switch

  return m_vSteeringForce;
}

//------------------------- ForwardComponent -----------------------------
//
//  returns the forward oomponent of the steering force
//------------------------------------------------------------------------
double Flocking::ForwardComponent()
{
  return m_pVehicle->Heading().Dot(m_vSteeringForce);
}

//--------------------------- SideComponent ------------------------------
//  returns the side component of the steering force
//------------------------------------------------------------------------
double Flocking::SideComponent()
{
  return m_pVehicle->Side().Dot(m_vSteeringForce);
}


//--------------------- AccumulateForce ----------------------------------
//
//  This function calculates how much of its max steering force the 
//  vehicle has left to apply and then applies that amount of the
//  force to add.
//------------------------------------------------------------------------
bool Flocking::AccumulateForce(Vector2D &RunningTot,
                                       Vector2D ForceToAdd)
{
  
  //calculate how much steering force the vehicle has used so far
  double MagnitudeSoFar = RunningTot.Length();

  //calculate how much steering force remains to be used by this vehicle
  double MagnitudeRemaining = m_pVehicle->MaxForce() - MagnitudeSoFar;

  //return false if there is no more force left to use
  if (MagnitudeRemaining <= 0.0) return false;

  //calculate the magnitude of the force we want to add
  double MagnitudeToAdd = ForceToAdd.Length();
  
  //if the magnitude of the sum of ForceToAdd and the running total
  //does not exceed the maximum force available to this vehicle, just
  //add together. Otherwise add as much of the ForceToAdd vector is
  //possible without going over the max.
  if (MagnitudeToAdd < MagnitudeRemaining)
  {
    RunningTot += ForceToAdd;
  }

  else
  {
    //add it to the steering force
    RunningTot += (Vec2DNormalize(ForceToAdd) * MagnitudeRemaining); 
  }

  return true;
}



//---------------------- CalculatePrioritized ----------------------------
//
//  this method calls each active steering behavior in order of priority
//  and acumulates their forces until the max steering force magnitude
//  is reached, at which time the function returns the steering force 
//  accumulated to that  point
//------------------------------------------------------------------------
Vector2D Flocking::CalculatePrioritized()
{       
  Vector2D force;
   
  //these next three can be combined for flocking behavior (wander is
  //also a good behavior to add into this mix)
  if (On(separation))
  {
	force = Separation(m_pVehicle->World()->Agents()) * m_dWeightSeparation;

	if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
  }

  if (On(allignment))
  {
	 force = Alignment(m_pVehicle->World()->Agents()) * m_dWeightAlignment;

	 if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
  }

  if (On(cohesion))
  {
     force = Cohesion(m_pVehicle->World()->Agents()) * m_dWeightCohesion;

	 if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce;
  }

  return m_vSteeringForce;
}


//---------------------- CalculateWeightedSum ----------------------------
//
//  this simply sums up all the active behaviors X their weights and 
//  truncates the result to the max available steering force before 
//  returning
//------------------------------------------------------------------------
Vector2D Flocking::CalculateWeightedSum()
{        
  
  if (On(separation))
  {
    m_vSteeringForce += Separation(m_pVehicle->World()->Agents()) * m_dWeightSeparation;
  }

  if (On(allignment))
  {
    m_vSteeringForce += Alignment(m_pVehicle->World()->Agents()) * m_dWeightAlignment;
  }

  if (On(cohesion))
  {
    m_vSteeringForce += Cohesion(m_pVehicle->World()->Agents()) * m_dWeightCohesion;
  }
  
  m_vSteeringForce.Truncate(m_pVehicle->MaxForce());
 
  return m_vSteeringForce;
}


//---------------------- CalculateDithered ----------------------------
//
//  this method sums up the active behaviors by assigning a probabilty
//  of being calculated to each behavior. It then tests the first priority
//  to see if it should be calcukated this simulation-step. If so, it
//  calculates the steering force resulting from this behavior. If it is
//  more than zero it returns the force. If zero, or if the behavior is
//  skipped it continues onto the next priority, and so on.
//
//  NOTE: Not all of the behaviors have been implemented in this method,
//        just a few, so you get the general idea
//------------------------------------------------------------------------
Vector2D Flocking::CalculateDithered()
{  
  //reset the steering force
   m_vSteeringForce.Zero();

  
    if (On(separation) && RandFloat() < 0.2)
    {
      m_vSteeringForce += Separation(m_pVehicle->World()->Agents()) * 
                          m_dWeightSeparation / 0.2;

      if (!m_vSteeringForce.isZero())
      {
        m_vSteeringForce.Truncate(m_pVehicle->MaxForce()); 
      
        return m_vSteeringForce;
      }
    }
  
    if (On(allignment) && RandFloat() < 0.3)
    {
      m_vSteeringForce += Alignment(m_pVehicle->World()->Agents()) *
                          m_dWeightAlignment / 0.3;

      if (!m_vSteeringForce.isZero())
      {
        m_vSteeringForce.Truncate(m_pVehicle->MaxForce()); 
      
        return m_vSteeringForce;
      }
    }

    if (On(cohesion) && RandFloat() < 0.6)
    {
      m_vSteeringForce += Cohesion(m_pVehicle->World()->Agents()) * 
                          m_dWeightCohesion / 0.6;

      if (!m_vSteeringForce.isZero())
      {
        m_vSteeringForce.Truncate(m_pVehicle->MaxForce()); 
      
        return m_vSteeringForce;
      }
    }
   
  return m_vSteeringForce;
}



/////////////////////////////////////////////////////////////////////////////// START OF BEHAVIORS


//---------------------------- Separation --------------------------------
//
// this calculates a force repelling from the other neighbors
//------------------------------------------------------------------------
Vector2D Flocking::Separation(const vector<Vehicle*> &neighbors)
{  
  Vector2D SteeringForce;

  for (unsigned int a=0; a<neighbors.size(); ++a)
  {
    //make sure this agent isn't included in the calculations and that
    //the agent being examined is close enough. ***also make sure it doesn't
    //include the evade target ***
    if(neighbors[a] != m_pVehicle)
    {
      Vector2D ToAgent = Vector2D(m_pVehicle->GetPosX(), m_pVehicle->GetPosY()) - Vector2D(neighbors[a]->GetPosX(), neighbors[a]->GetPosY());

      //scale the force inversely proportional to the agents distance  
      //from its neighbor.
      SteeringForce += Vec2DNormalize(ToAgent)/ToAgent.Length();
    }
  }

  return SteeringForce;
}


//---------------------------- Alignment ---------------------------------
//
//  returns a force that attempts to align this agents heading with that
//  of its neighbors
//------------------------------------------------------------------------
Vector2D Flocking::Alignment(const vector<Vehicle*>& neighbors)
{
  //used to record the average heading of the neighbors
  Vector2D AverageHeading;

  //used to count the number of vehicles in the neighborhood
  int    NeighborCount = 0;

  //iterate through all the tagged vehicles and sum their heading vectors  
  for (unsigned int a=0; a<neighbors.size(); ++a)
  {
    //make sure *this* agent isn't included in the calculations and that
    //the agent being examined  is close enough ***also make sure it doesn't
    //include any evade target ***
    if(neighbors[a] != m_pVehicle)
    {
      AverageHeading += neighbors[a]->Heading();

      ++NeighborCount;
    }
  }

  //if the neighborhood contained one or more vehicles, average their
  //heading vectors.
  if (NeighborCount > 0)
  {
    AverageHeading /= (double)NeighborCount;

    AverageHeading -= m_pVehicle->Heading();
  }
  
  return AverageHeading;
}

//-------------------------------- Cohesion ------------------------------
//
//  returns a steering force that attempts to move the agent towards the
//  center of mass of the agents in its immediate area
//------------------------------------------------------------------------
Vector2D Flocking::Cohesion(const vector<Vehicle*> &neighbors)
{
  //first find the center of mass of all the agents
  Vector2D CenterOfMass, SteeringForce;

  int NeighborCount = 0;

  //iterate through the neighbors and sum up all the position vectors
  for (unsigned int a=0; a<neighbors.size(); ++a)
  {
    //make sure *this* agent isn't included in the calculations and that
    //the agent being examined is close enough ***also make sure it doesn't
    //include the evade target ***
    if(neighbors[a] != m_pVehicle)
    {
      CenterOfMass += Vector2D(neighbors[a]->GetPosX(), neighbors[a]->GetPosY());

      ++NeighborCount;
    }
  }

  if (NeighborCount > 0)
  {
    //the center of mass is the average of the sum of positions
    //CenterOfMass /= (double)NeighborCount;
	  CenterOfMass = Vector2D(EntityManager::Instance()->GetEntityByID(ent_Peon)->GetPosX(), EntityManager::Instance()->GetEntityByID(ent_Peon)->GetPosY());

	
    //now seek towards that position
    
	Vector2D DesiredVelocity = Vec2DNormalize(CenterOfMass - Vector2D(m_pVehicle->GetPosX(), m_pVehicle->GetPosY())) * m_pVehicle->MaxSpeed();

	SteeringForce = DesiredVelocity - m_pVehicle->Velocity();
  }

  //the magnitude of cohesion is usually much larger than separation or
  //allignment so it usually helps to normalize it.
  return Vec2DNormalize(SteeringForce);
}
